package program;

import javax.vecmath.Vector3d;
import simbad.sim.Agent;
import simbad.sim.CameraSensor;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;

class MyRobot extends Agent {
   
    RangeSensorBelt sonars;
    CameraSensor camera;
   
    private double initSpeed;
    private int sensorIndex;
   
    public MyRobot (Vector3d position, String name, double initSpeed) {    
        super(position,name);
       
        camera = RobotFactory.addCameraSensor(this);
        // Add sonars
        sonars = RobotFactory.addSonarBeltSensor(this);
       
        this.initSpeed = initSpeed;
        sensorIndex = initSpeed > 0 ? 0 : 3;
    }
    public void initBehavior() {
        setTranslationalVelocity(initSpeed);
    }
   
    public void performBehavior() {
       
        double front = sonars.getFrontQuadrantMeasurement();
        double back = sonars.getBackQuadrantMeasurement();
        double left = sonars.getBackLeftQuadrantMeasurement();
        double right = sonars.getBackRightQuadrantMeasurement();
       
        //sonars.getMeasurement(sensorIndex);
       
        /*if(getCounter() % 100 == 0) {
            System.out.println(getName()+" :"+measure);
           
        }*/
       
       
        if (front < 1 || back < 1 || left < 1 || right < 1) {
            System.out.println(getName()+" :"+front);
            setRotationalVelocity(Math.random()*10);
           
        }else {
            setRotationalVelocity(0.0);
        }
        if(collisionDetected()) {
            initSpeed = -initSpeed;
            setTranslationalVelocity(initSpeed);
            //sensorIndex = initSpeed > 0 ? 0 : 3;
           
        }
       
    }
}